#include <gtest/gtest.h>
#include <string.h>
#include <string>
#include "ini/inisafezone.hh"

class SafeZoneTest : public testing::Test
{
public:
    std::string path = "src/safezone/test/testsafezone2.xml";
    SafeZone saveZone;
    SafeZone loadZone;

public:
    virtual void SetUp() override
    {
        std::string cmd = "rm -f " + path;
        system(cmd.c_str());
    }
    virtual void TearDown() override {}
    SafeZoneTest()
    {
        memset(&this->loadZone, 0, sizeof(this->loadZone));
        memset(&this->saveZone, 0, sizeof(this->saveZone));
        this->iniSaveZoneData(this->saveZone);
    }
    void iniSaveZoneData(SafeZone& safeZone);
};

void SafeZoneTest::iniSaveZoneData(SafeZone& safeZone)
{
    /**
     * 平面外参数写入
    */
    safeZone.openEnable = 1;
    safeZone.autoEnable = 0;
    safeZone.safeZoneMode = 2;
    safeZone.elbow_pos_limit_enable = 1;
    safeZone.wrist_pos_limit_enable = 1;
    safeZone.safe = 1;

    /**
     * 平面1测试正常的各个参数写入
    */
    safeZone.zone.plane[0].modify = 0;
    safeZone.zone.plane[0].enable = 0;
    strcpy(safeZone.zone.plane[0].name, "平面1");
    safeZone.zone.plane[0].planePoints[0].point.x = 1.2356;
    safeZone.zone.plane[0].planePoints[0].point.y = 2.2356;
    safeZone.zone.plane[0].planePoints[0].point.z = 3.2356;
    safeZone.zone.plane[0].planePoints[0].rpy.y = 4.2356;
    safeZone.zone.plane[0].planePoints[0].rpy.p = 5.2356;
    safeZone.zone.plane[0].planePoints[0].rpy.r = 6.2356;
    safeZone.zone.plane[0].planePoints[1].point.x = 10.2356;
    safeZone.zone.plane[0].planePoints[1].point.y = 2.2356;
    safeZone.zone.plane[0].planePoints[1].point.z = 3.2356;
    safeZone.zone.plane[0].planePoints[1].rpy.y = 4.2356;
    safeZone.zone.plane[0].planePoints[1].rpy.p = 5.2356;
    safeZone.zone.plane[0].planePoints[1].rpy.r = 6.2356;
    safeZone.zone.plane[0].planePoints[2].point.x = 10.2356;
    safeZone.zone.plane[0].planePoints[2].point.y = 20.2356;
    safeZone.zone.plane[0].planePoints[2].point.z = 3.2356;
    safeZone.zone.plane[0].planePoints[2].rpy.y = 4.2356;
    safeZone.zone.plane[0].planePoints[2].rpy.p = 5.2356;
    safeZone.zone.plane[0].planePoints[2].rpy.r = 6.2356;
    safeZone.zone.plane[0].safeD = 100.326;
    /**
     * 平面2测试 平面点3点共线 
    */
    safeZone.zone.plane[1].modify = 0;
    safeZone.zone.plane[1].planePoints[0].point.x = 1.2356;
    safeZone.zone.plane[1].planePoints[0].point.y = 2.2356;
    safeZone.zone.plane[1].planePoints[0].point.z = 3.2356;
    safeZone.zone.plane[1].planePoints[0].rpy.y = 4.2356;
    safeZone.zone.plane[1].planePoints[0].rpy.p = 5.2356;
    safeZone.zone.plane[1].planePoints[0].rpy.r = 6.2356;
    safeZone.zone.plane[1].planePoints[1].point.x = 10.2356;
    safeZone.zone.plane[1].planePoints[1].point.y = 2.2356;
    safeZone.zone.plane[1].planePoints[1].point.z = 3.2356;
    safeZone.zone.plane[1].planePoints[1].rpy.y = 4.2356;
    safeZone.zone.plane[1].planePoints[1].rpy.p = 5.2356;
    safeZone.zone.plane[1].planePoints[1].rpy.r = 6.2356;
    safeZone.zone.plane[1].planePoints[2].point.x = 100.2356;
    safeZone.zone.plane[1].planePoints[2].point.y = 2.2356;
    safeZone.zone.plane[1].planePoints[2].point.z = 3.2356;
    safeZone.zone.plane[1].planePoints[2].rpy.y = 4.2356;
    safeZone.zone.plane[1].planePoints[2].rpy.p = 5.2356;
    safeZone.zone.plane[1].planePoints[2].rpy.r = 6.2356;
    /**
     * 平面3测试安全面点在安全平面上的报错
    */
    safeZone.zone.plane[2].modify = 0;
    safeZone.zone.plane[2].planePoints[0].point.x = 1.2356;
    safeZone.zone.plane[2].planePoints[0].point.y = 2.2356;
    safeZone.zone.plane[2].planePoints[0].point.z = 3.2356;
    safeZone.zone.plane[2].planePoints[0].rpy.y = 4.2356;
    safeZone.zone.plane[2].planePoints[0].rpy.p = 5.2356;
    safeZone.zone.plane[2].planePoints[0].rpy.r = 6.2356;
    safeZone.zone.plane[2].planePoints[1].point.x = 10.2356;
    safeZone.zone.plane[2].planePoints[1].point.y = 2.2356;
    safeZone.zone.plane[2].planePoints[1].point.z = 3.2356;
    safeZone.zone.plane[2].planePoints[1].rpy.y = 4.2356;
    safeZone.zone.plane[2].planePoints[1].rpy.p = 5.2356;
    safeZone.zone.plane[2].planePoints[1].rpy.r = 6.2356;
    safeZone.zone.plane[2].planePoints[2].point.x = 10.2356;
    safeZone.zone.plane[2].planePoints[2].point.y = 20.2356;
    safeZone.zone.plane[2].planePoints[2].point.z = 3.2356;
    safeZone.zone.plane[2].planePoints[2].rpy.y = 4.2356;
    safeZone.zone.plane[2].planePoints[2].rpy.p = 5.2356;
    safeZone.zone.plane[2].planePoints[2].rpy.r = 6.2356;
    safeZone.zone.plane[2].safePoint.point.x = 1.2356;
    safeZone.zone.plane[2].safePoint.point.y = 200.2356;
    safeZone.zone.plane[2].safePoint.point.z = 3.2356;
    safeZone.zone.plane[2].safePoint.rpy.y = 4.2356;
    safeZone.zone.plane[2].safePoint.rpy.p = 5.2356;
    safeZone.zone.plane[2].safePoint.rpy.r = 6.2356;
    /**
     * 平面4测试modify没有置位，进行平面信息写入
    */
    safeZone.zone.plane[3].modify = 0;
    safeZone.zone.plane[3].enable = 0;
    strcpy(safeZone.zone.plane[3].name, "平面4");
    safeZone.zone.plane[3].planePoints[0].point.x = 1.2356;
    safeZone.zone.plane[3].planePoints[0].point.y = 2.2356;
    safeZone.zone.plane[3].planePoints[0].point.z = 3.2356;
    safeZone.zone.plane[3].planePoints[0].rpy.y = 4.2356;
    safeZone.zone.plane[3].planePoints[0].rpy.p = 5.2356;
    safeZone.zone.plane[3].planePoints[0].rpy.r = 6.2356;
    safeZone.zone.plane[3].safeD = 100.326;
}

TEST_F(SafeZoneTest, noFileLoad)
{
    SafeZone zone;
    memset(&zone, 0, sizeof(zone));
    /**
     * 配置文件丢失时读取配置
    */
    EXPECT_EQ(SafeZoneConfig::loadSafeZone(path.c_str(), zone), -1);
}

TEST_F(SafeZoneTest, noFileSave)
{
    /**
     * 配置文件丢失时写入配置
    */
    SafeZone zone;
    memset(&zone, 0, sizeof(zone));
    EXPECT_EQ(SafeZoneConfig::saveSafeZone(this->path.c_str(), zone), 0);
    EXPECT_EQ(SafeZoneConfig::loadSafeZone(this->path.c_str(), zone), 0);
}

#define SAFEZONE_SAVE_AND_LOAD_TEST(planeNumber)                                                 \
    do {                                                                                         \
        EXPECT_EQ(SafeZoneConfig::saveSafeZone(this->path.c_str(), this->saveZone), 0);          \
        ASSERT_EQ(SafeZoneConfig::loadSafeZone(this->path.c_str(), this->loadZone), 0);          \
        EXPECT_EQ(this->loadZone.openEnable, 1);                                                 \
        EXPECT_EQ(this->loadZone.autoEnable, 0);                                                 \
        EXPECT_EQ(this->loadZone.elbow_pos_limit_enable, 1);                                     \
        EXPECT_EQ(this->loadZone.wrist_pos_limit_enable, 1);                                     \
        EXPECT_EQ(this->loadZone.safeZoneMode, 2);                                               \
        EXPECT_EQ(this->loadZone.zone.plane[planeNumber].enable, 0);                             \
        EXPECT_STREQ(this->loadZone.zone.plane[planeNumber].name, "平面1");                      \
        EXPECT_DOUBLE_EQ(this->loadZone.zone.plane[planeNumber].planePoints[0].point.x, 1.2356); \
        EXPECT_DOUBLE_EQ(this->loadZone.zone.plane[planeNumber].planePoints[0].point.y, 2.2356); \
        EXPECT_DOUBLE_EQ(this->loadZone.zone.plane[planeNumber].planePoints[0].point.z, 3.2356); \
        EXPECT_DOUBLE_EQ(this->loadZone.zone.plane[planeNumber].planePoints[0].rpy.y, 4.2356);   \
        EXPECT_DOUBLE_EQ(this->loadZone.zone.plane[planeNumber].planePoints[0].rpy.p, 5.2356);   \
        EXPECT_DOUBLE_EQ(this->loadZone.zone.plane[planeNumber].planePoints[0].rpy.r, 6.2356);   \
        EXPECT_DOUBLE_EQ(this->loadZone.zone.plane[planeNumber].safeD, 100.326);                 \
    } while (0)

#define PLANE_POINT_CHECK_TEST(planeNumber)                                                                                                       \
    do {                                                                                                                                          \
        EXPECT_EQ(SafeZoneConfig::saveSafeZone(this->path.c_str(), this->saveZone), -1);                                                          \
        EXPECT_EQ(SafeZoneConfig::loadSafeZone(this->path.c_str(), this->loadZone), 0);                                                           \
        EXPECT_DOUBLE_EQ(this->loadZone.zone.plane[planeNumber].planePoints[0].point.x, lastZone.zone.plane[planeNumber].planePoints[0].point.x); \
        EXPECT_DOUBLE_EQ(this->loadZone.zone.plane[planeNumber].planePoints[0].point.y, lastZone.zone.plane[planeNumber].planePoints[0].point.y); \
        EXPECT_DOUBLE_EQ(this->loadZone.zone.plane[planeNumber].planePoints[0].point.z, lastZone.zone.plane[planeNumber].planePoints[0].point.z); \
        EXPECT_DOUBLE_EQ(this->loadZone.zone.plane[planeNumber].planePoints[0].rpy.y, lastZone.zone.plane[planeNumber].planePoints[0].rpy.y);     \
        EXPECT_DOUBLE_EQ(this->loadZone.zone.plane[planeNumber].planePoints[0].rpy.p, lastZone.zone.plane[planeNumber].planePoints[0].rpy.p);     \
        EXPECT_DOUBLE_EQ(this->loadZone.zone.plane[planeNumber].planePoints[0].rpy.r, lastZone.zone.plane[planeNumber].planePoints[0].rpy.r);     \
        EXPECT_DOUBLE_EQ(this->loadZone.zone.plane[planeNumber].safeD, lastZone.zone.plane[1].safeD);                                             \
    } while (0)

#define PLANE_SAFEPOINT_CHECK_TEST(planeNumber)                                                                                                   \
    do {                                                                                                                                          \
        EXPECT_EQ(SafeZoneConfig::saveSafeZone(this->path.c_str(), this->saveZone), -1);                                                          \
        EXPECT_EQ(SafeZoneConfig::loadSafeZone(this->path.c_str(), this->loadZone), 0);                                                           \
        EXPECT_DOUBLE_EQ(this->loadZone.zone.plane[planeNumber].planePoints[0].point.x, lastZone.zone.plane[planeNumber].planePoints[0].point.x); \
        EXPECT_DOUBLE_EQ(this->loadZone.zone.plane[planeNumber].planePoints[0].point.y, lastZone.zone.plane[planeNumber].planePoints[0].point.y); \
        EXPECT_DOUBLE_EQ(this->loadZone.zone.plane[planeNumber].planePoints[0].point.z, lastZone.zone.plane[planeNumber].planePoints[0].point.z); \
        EXPECT_DOUBLE_EQ(this->loadZone.zone.plane[planeNumber].planePoints[0].rpy.y, lastZone.zone.plane[planeNumber].planePoints[0].rpy.y);     \
        EXPECT_DOUBLE_EQ(this->loadZone.zone.plane[planeNumber].planePoints[0].rpy.p, lastZone.zone.plane[planeNumber].planePoints[0].rpy.p);     \
        EXPECT_DOUBLE_EQ(this->loadZone.zone.plane[planeNumber].planePoints[0].rpy.r, lastZone.zone.plane[planeNumber].planePoints[0].rpy.r);     \
        EXPECT_DOUBLE_EQ(this->loadZone.zone.plane[planeNumber].safeD, lastZone.zone.plane[planeNumber].safeD);                                   \
    } while (0)
TEST_F(SafeZoneTest, safeZoneTest)
{
    this->path = "src/safezone/test/testsafezone.xml";
    /**
     * 平面1测试
    */
    this->saveZone.zone.plane[0].modify = 1;
    SAFEZONE_SAVE_AND_LOAD_TEST(0);
    /**
     * 保存当前配置信息
    */
    SafeZone lastZone;
    memset(&lastZone, 0, sizeof(lastZone));
    ASSERT_EQ(SafeZoneConfig::loadSafeZone(this->path.c_str(), lastZone), 0);

    /**
     * 平面2测试  安全平面三点共线
    */
    this->saveZone.zone.plane[0].modify = 0;
    this->saveZone.zone.plane[1].modify = 1;
    PLANE_POINT_CHECK_TEST(1);
    /**
     * 平面3测试  安全面点在安全平面上
    */
    this->saveZone.zone.plane[1].modify = 0;
    this->saveZone.zone.plane[2].modify = 1;
    PLANE_SAFEPOINT_CHECK_TEST(2);
}
